/**
 * ...
 * @author Krzysztof Rozalski
 */

package hapi.physics.integration.test;
import hapi.math.MathUtil;

typedef State = {
	var x:Float;
	var v:Float;
}

typedef Derivative = {
	var dx:Float;
	var dv:Float;
}

class EulerVsRK4 {
	
	static var V_LIMIT = 61;

	static function accelerationSpring(state:State, t:Float) {
		var k = 10;
		var b = 1;
		return - k*state.x - b*state.v;
	}
	
	static function accelerationCar(state:State, t:Float) {
		var maxV = 61;
		var maxDelta = 10;
		if ( state.v >= maxV )
			return 0.0;
		else
			return ((maxV - state.v) / maxV) * maxDelta;
	}
	
	static function accelerationCarConst(state:State, t:Float) {
		return 10.0;//
	}
	
	static function acceleration(state:State, t:Float) {
		//return accelerationSpring(state, t);
		return accelerationCarConst(state, t);
	}

	static function evaluateWithoutDerivative(initial:State, t:Float):Derivative {
		var output:Derivative = {
			dx : initial.v,
			dv : acceleration(initial, t)
		};
		return output;
	}

	static function evaluate(initial:State, t:Float, dt:Float, d:Derivative):Derivative {
		var state:State = {
			x: initial.x + d.dx*dt,
			v: initial.v + d.dv*dt
		};
		var output:Derivative = {
			dx : state.v,
			dv : acceleration(state, t + dt)
		};
		return output;
	}

	static function integrateRK4(state:State, dt:Float, t:Float) {
		var a = evaluateWithoutDerivative(state, t);
		var b = evaluate(state, t, dt*0.5, a);
		var c = evaluate(state, t, dt*0.5, b);
		var d = evaluate(state, t, dt, c);
		trace(Std.string(a) + " " + Std.string(b) + " " + Std.string(c) + " " + Std.string(d));
		var dxdt:Float = 1.0/6.0 * (a.dx + 2.0*(b.dx + c.dx) + d.dx);
		var dvdt:Float = 1.0/6.0 * (a.dv + 2.0*(b.dv + c.dv) + d.dv);
		//trace(dxdt + " <-> " + dvdt);
		state.x = state.x + dxdt*dt;
		state.v = state.v + dvdt*dt;
	}
	
	static function integrateEuler(state:State, dt:Float, t:Float ) {
		var acc = acceleration(state, t + dt);
		state.x = state.x + state.v * dt;
		state.v = state.v + acc*dt;
	}

	public static function main() {
		var state:State = {
			x:0.0,
			v:0.0
		};
		
		var state2:State = {
			x:0.0,
			v:0.0
		};
		
		var t = 0.;
		var dt = 1;
		#if neko
			haxe.Log.trace = function(msg:Dynamic, ?infos) { neko.Lib.println(msg); }
		#end
		var i = 0;
		var px = 0.;
		var pv = 0.;
		var r = MathUtil.roundTo;
		while (t < 10) {
			integrateRK4(state, dt, t);
			integrateEuler(state2, dt, t);
			t += dt;
			//trace(r(state.x,2)+" "+r(state.v,2)+" "+r(state.x-px,2)+" "+r(state.v-pv,2));
			trace(t+": "+r(state.x,2)+" "+r(state.v,2)+" vs "+r(state2.x,2)+" "+r(state2.v,2));
			px = state.x;
			pv = state.v;
			++i;
		}
		return 0;
	}
}